import sensor, image, time, lcd, gc, cmath
import _thread
from fpioa_manager import fm
from machine import UART
from modules import ybkey
yb_uart = UART(UART.UART2, 9600, 8, 0, 0, timeout=2000, read_buf_len=4096)
write_bytes4 = [0xFF, 0x02, 0x00, 0x00, 0x00, 0x08, 0x6C, 0x12]#读取1号光耦输入状态
write_bytes5 = [0xFF, 0x02, 0x00, 0x01, 0x00, 0x08, 0x3D, 0xD2]#读取2号光耦输入状态
write_bytes11 = [0xFF, 0x05, 0x00, 0x00, 0xFF, 0x00, 0x99, 0xE4]#打开1继电器
write_bytes12 = [0xFF, 0x05, 0x00, 0x00, 0x00, 0x00, 0xD8, 0x14]#关闭1继电器
write_bytes21 = [0xFF, 0x02, 0x01, 0x00, 0x90, 0x60]#1号光耦off
write_bytes22 = [0xFF, 0x02, 0x01, 0x01, 0x51, 0xA0]#1号光耦on
write_bytes31 = [0xFF, 0x05, 0x00, 0x01, 0xFF, 0x00, 0xC8, 0x24]#打开2继电器
write_bytes32 = [0xFF, 0x05, 0x00, 0x01, 0x00, 0x00, 0x89, 0xD4]#关闭2继电器
write_bytes41 = [0xFF, 0x02, 0x01, 0x00, 0x90, 0x60]#2号光耦off,还没读取验证
write_bytes42 = [0xFF, 0x02, 0x01, 0x02, 0x11, 0xA1]#2号光耦on，还没读取验证
last_time = time.ticks_ms()
gc.collect()
lcd.init()


sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing((320, 240))
sensor.run(1)
sensor.skip_frames(10)

KEY=ybkey()
try:
    template = image.Image("/sd/tt.jpg")
    template_gray = template.to_grayscale(0)
    print("exist")
except:
    img=sensor.snapshot()
    img_roi=img.copy((120,70,90,90))
    img_roi.save("/sd/tt.jpg")
    template = image.Image("/sd/tt.jpg")
    template_gray = template.to_grayscale(0)
    print("set")

clock = time.clock()
while(True):
    img=sensor.snapshot()
    img_1=img.copy()
    img_1.draw_rectangle((120,70,90,90),(0,0,255),2)
    if time.ticks_ms() - last_time >= 10:
        last_time = time.ticks_ms()
        yb_uart.write(bytes(write_bytes4))
        time.sleep_ms(10)
        #if True:#yb_uart.any()
        if yb_uart.any():
           read_data = yb_uart.read()
           #if True:#read_data
           if read_data:
                #if True:#bytes(write_bytes22) == read_data
                if bytes(write_bytes22) == read_data:
                    img_gray = img.to_grayscale(0)
                    data = img_gray.find_template(template_gray, 0.85, (110,60,110,110), 2, image.SEARCH_EX )
                    if data != None:
                        img_1.draw_rectangle(data,(0,255,0),2)
                        img_1.draw_string(110, 0,"right",color=(0,255,0),scale=4,mono_space=False)
                        yb_uart.write(bytes(write_bytes11))
                        lcd.display(img_1)
                        time.sleep_ms(1000)
                        yb_uart.write(bytes(write_bytes12))
                        print(data)
                    else:
                        img_1.draw_string(100, 0,"wrong",color=(255,0,0),scale=4,mono_space=False)
                        img_1.draw_rectangle((120,70,90,90),(255,0,0),2)
    state=KEY.is_press()
    if (state==1):
       time.sleep_ms(5)
       state=KEY.is_press()
       if(state==1):
            img_roi=img.copy((120,70,90,90))
            img_roi.save("/sd/tt.jpg")
            template = image.Image("/sd/tt.jpg")
            template_gray = template.to_grayscale(0)
            print("success")
            img_1.draw_string(60, 200,"set_tempalte",color=(0,255,0),scale=3,mono_space=False)
            lcd.display(img_1)
    lcd.display(img_1)


